package es.uji.viselab.robot.links.fanuc;

import java.io.ByteArrayInputStream;
import java.io.IOException;
import java.text.DecimalFormat;
import java.text.DecimalFormatSymbols;
import java.text.ParseException;

import javax.xml.bind.annotation.XmlRootElement;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import org.w3c.dom.Element;
import org.xml.sax.SAXException;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.robot.RobotException;

@XmlRootElement(name = "RobotPosition", namespace = "")
public class RobotPosition {

	private double joint1;
	private double joint2;
	private double joint3;
	private double joint4;
	private double joint5;
	private double joint6;
	private long x;
	private long y;
	private long z;
	private long w;
	private long p;
	private long r;

	public RobotPosition() {

	}

	public double getJoint1() {
		return joint1;
	}

	public double getJoint2() {
		return joint2;
	}

	public double getJoint3() {
		return joint3;
	}

	public double getJoint4() {
		return joint4;
	}

	public double getJoint5() {
		return joint5;
	}

	public double getJoint6() {
		return joint6;
	}

	public long getX() {
		return x;
	}

	public void setX(long x) {
		this.x = x;
	}

	public long getY() {
		return y;
	}

	public void setY(long y) {
		this.y = y;
	}

	public long getZ() {
		return z;
	}

	public void setZ(long z) {
		this.z = z;
	}

	public long getW() {
		return w;
	}

	public void setW(long w) {
		this.w = w;
	}

	public long getP() {
		return p;
	}

	public void setP(long p) {
		this.p = p;
	}

	public long getR() {
		return r;
	}

	public void setR(long r) {
		this.r = r;
	}

	private long stringToLong(String stringValue) throws RobotException {
		Number numberValue;
		Long longValue;
		DecimalFormatSymbols tailoredFormat = new DecimalFormatSymbols();
		tailoredFormat.setDecimalSeparator(',');
		String mask = "#########,#############";
		DecimalFormat df = new DecimalFormat(mask, tailoredFormat);
		try {
			numberValue = df.parse(stringValue);
		} catch (ParseException e) {
			throw new RobotException(e.getMessage());
		}
		longValue = numberValue.longValue();
		return longValue;
	}

	public RobotPosition(String xml) throws RobotException {

		Element node;
		PointXYZ locationxyz = new PointXYZ();
		PointXYZ orientationxyz = new PointXYZ();
		Joints joint = new Joints();

		try {

			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			locationxyz.setX(stringToLong(node.getElementsByTagName("x").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setY(stringToLong(node.getElementsByTagName("y").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setZ(stringToLong(node.getElementsByTagName("z").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setX(stringToLong(node.getElementsByTagName("w").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setY(stringToLong(node.getElementsByTagName("p").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setZ(stringToLong(node.getElementsByTagName("r").item(0).getFirstChild()
					.getNodeValue()));
			this.x = (long) locationxyz.getX();
			this.y = (long) locationxyz.getY();
			this.z = (long) locationxyz.getZ();
			this.w = (long) orientationxyz.getX();
			this.p = (long) orientationxyz.getY();
			this.r = (long) orientationxyz.getZ();

		} catch (SAXException e) {
			throw new RobotException(e.getMessage());
		} catch (IOException e) {
			throw new RobotException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotException(e.getMessage());
		}

		try {

			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			joint.setJoint1(Integer.parseInt(node.getElementsByTagName("joint1").item(0)
					.getFirstChild().getNodeValue()));
			joint.setJoint2(Integer.parseInt(node.getElementsByTagName("joint2").item(0)
					.getFirstChild().getNodeValue()));
			joint.setJoint3(Integer.parseInt(node.getElementsByTagName("joint3").item(0)
					.getFirstChild().getNodeValue()));
			joint.setJoint4(Integer.parseInt(node.getElementsByTagName("joint4").item(0)
					.getFirstChild().getNodeValue()));
			joint.setJoint5(Integer.parseInt(node.getElementsByTagName("joint5").item(0)
					.getFirstChild().getNodeValue()));
			joint.setJoint6(Integer.parseInt(node.getElementsByTagName("joint6").item(0)
					.getFirstChild().getNodeValue()));
			this.joint1 = (int) joint.getJoint1();
			this.joint2 = (int) joint.getJoint2();
			this.joint3 = (int) joint.getJoint3();
			this.joint4 = (int) joint.getJoint4();
			this.joint5 = (int) joint.getJoint5();
			this.joint6 = (int) joint.getJoint6();

		} catch (SAXException e) {
			throw new RobotException(e.getMessage());
		} catch (IOException e) {
			throw new RobotException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotException(e.getMessage());
		}

	}

	public void setJoint1(double j) {
		this.joint1 = j;
	}

	public void setJoint2(double j) {
		this.joint2 = j;
	}

	public void setJoint3(double j) {
		this.joint3 = j;
	}

	public void setJoint4(double j) {
		this.joint4 = j;
	}

	public void setJoint5(double j) {
		this.joint5 = j;
	}

	public void setJoint6(double j) {
		this.joint6 = j;
	}

}
